#include "main.h"
#include "spi.h"
int16_t imu963ra_gyro_x = 0, imu963ra_gyro_y = 0, imu963ra_gyro_z = 0;
int16_t imu963ra_acc_x = 0, imu963ra_acc_y = 0, imu963ra_acc_z = 0;
int16_t imu963ra_mag_x = 0, imu963ra_mag_y = 0, imu963ra_mag_z = 0;
float imu963ra_transition_factor[3] = {4098, 14.3, 12000};
imu963_t imu963;
// 实时调参
float factor_gyro_lvbo = 0.15f; // 一阶低通滤波参数 一阶低通滤波器： 毛刺多就减少，失真就增大
// 上海重力加速度9.7963  亳州9.7903
#define Gg 9.7903
void get_gyro_sec(imu963_t *imu)
{
	for (uint8_t i = 0; i < 3; i++)
	{
		imu->gyro_real_sec[i] = imu->gyro_real[i] * 3.14 / 180;
	}
}
// 获取零漂移
void get_imu_offset(imu963_t *imu)
{
	uint16_t n = 200;
	while (n)
	{
		imu963ra_get_acc();
		imu->acc_offset[0] += (imu963ra_acc_x) / 100 * 100;
		imu->acc_offset[1] += (imu963ra_acc_y) / 100 * 100;
		imu->acc_offset[2] += ((imu963ra_acc_z)-4098) / 100 * 100;
		imu963ra_get_gyro();
		imu->gyro_offset[0] += (imu963ra_gyro_x) / 10 * 10;
		imu->gyro_offset[1] += (imu963ra_gyro_y) / 10 * 10;
		imu->gyro_offset[2] += (imu963ra_gyro_z) / 10 * 10;
		HAL_Delay(5);
		n--;
	}
	n = 200;
	for (uint8_t i = 0; i < 3; i++)
	{
		imu->acc_offset[i] /= n;
		imu->gyro_offset[i] /= n;
	}

	// OLED_ShowSignedNum(1, 8, imu->acc_offset[0], 3, OLED_6X8);
	// OLED_ShowSignedNum(1, 24, imu->acc_offset[1], 3, OLED_6X8);
	// OLED_ShowSignedNum(1, 24, imu->acc_offset[1], 3, OLED_6X8);
	// OLED_ShowSignedNum(1, 40, imu->acc_offset[2], 3, OLED_6X8);
	// OLED_ShowSignedNum(60, 8, imu->gyro_offset[0], 3, OLED_6X8);
	// OLED_ShowSignedNum(60, 24, imu->gyro_offset[1], 3, OLED_6X8);
	// OLED_ShowSignedNum(60, 40, imu->gyro_offset[2], 3, OLED_6X8);
}

// 滤波函数
void get_imu_filter(imu963_t *imu)
{
	// 获取加速计原始数据
	imu963ra_get_acc();
	// 去除零漂
	imu->acc[0] = (imu963ra_acc_x)-imu->acc_offset[0];
	imu->acc[1] = (imu963ra_acc_y)-imu->acc_offset[1];
	imu->acc[2] = (imu963ra_acc_z)-imu->acc_offset[2];

	for (uint8_t i = 0; i < 3; i++) // 对加速度原始数据进行一阶卡尔曼滤波
	{
		// lastp:输入值不可靠就调大	,Q 噪声大就调大 ，R和Q一样调
		static struct _1_ekf_filter ekf[3] = {{0.02, 0, 0, 0, 0.001, 0.543}, {0.03, 0, 0, 0, 0.0015, 0.545}, {0.02, 0, 0, 0, 0.001, 0.543}};
		kalman_1(&ekf[i], (float)(imu->acc[i]));
		imu->acc[i] = (uint16_t)ekf[i].out;
	}
	// 查看kaerman的效果
	// hhSerial_Printf("%d,%d,%d,%d,%d,%d\n", imu->acc[0], imu963ra_acc_x, imu->acc[1], imu963ra_acc_y, imu->acc[2], imu963ra_acc_z);
	// 获取陀螺仪数据
	imu963ra_get_gyro();
	imu->gyro[0] = (imu963ra_gyro_x)-imu->gyro_offset[0];
	imu->gyro[1] = (imu963ra_gyro_y)-imu->gyro_offset[1];
	imu->gyro[2] = (imu963ra_gyro_z)-imu->gyro_offset[2];
	for (uint8_t i = 0; i < 3; i++) // 对角速度原始数据进行一阶低通滤波
	{
		static float tBuff[3];
		imu->gyro[i] = tBuff[i] = tBuff[i] * (1 - factor_gyro_lvbo) + (float)(imu->gyro[i]) * factor_gyro_lvbo;
	}
	// hhSerial_Printf("%d,%d,%d,%d,%d,%d\n", imu->gyro[0], imu963ra_gyro_z, imu->gyro[1], imu963ra_gyro_y, imu->gyro[2], imu963ra_gyro_z);
}
void get_imu_real(imu963_t *imu)
{
	for (uint8_t i = 0; i < 3; i++)
	{
		imu->acc_real[i] = imu963ra_acc_transition(imu->acc[i]) * Gg;
		imu->gyro_real[i] = imu963ra_gyro_transition(imu->gyro[i]);
		imu->mag_real[i] = imu963ra_mag_transition(imu->mag[i]);
	}
}
// 获取imu数据并且做对应的滤波处理
void get_imu_data(imu963_t *imu)
{ // 在初始化的时候使用
	// get_imu_offset(imu);
	get_imu_filter(imu);
	get_imu_real(imu);
	get_gyro_sec(imu);
}

/******************************************************************************
 * 函  数：uint8_t MPU6050_OffSet(INT16_XYZ value,INT16_XYZ *offset,uint16_t sensivity)
 * 功  能：MPU6050零偏校准
 * 参  数：value： 	 MPU6050原始数据
 *         offset：	 校准后的零偏值
 *         sensivity：加速度计的灵敏度
 * 返回值：1校准完成 0校准未完成
 * 备  注：无
 *******************************************************************************/

/**
 * 函    数：MPU6050读取ID号
 * 参    数：MID 工厂ID，使用输出参数的形式返回
 * 参    数：DID 设备ID，使用输出参数的形式返回
 * 返 回 值：无
 */
uint8_t IMU_ReadID()
{
	uint8_t id;
	imu963ra_read_acc_gyro_register(IMU963RA_WHO_AM_I, &id);
	return id;
}
uint8_t regtext(uint8_t reg)
{
	uint8_t id;

	imu963ra_read_acc_gyro_register(reg, &id);
	return id;
}

void imu963ra_write_acc_gyro_register(uint8_t reg, uint8_t data)
{
	uint8_t dat[2];
	dat[0] = reg | IMU963RA_SPI_W;
	dat[1] = data;
	uint8_t date[2];
	MySPI_Start(); // SPI起始
	HAL_SPI_TransmitReceive(&hspi2, dat, date, 2, 500);
	MySPI_Stop(); // SPI起始
}
void imu963ra_read_acc_gyro_register(uint8_t reg, uint8_t *val)
{
	uint8_t tx_data[2] = {reg | IMU963RA_SPI_R, 0};
	uint8_t rx_data[2] = {0};

	MySPI_Start(); // 片选拉低
	HAL_SPI_TransmitReceive(&hspi2, tx_data, rx_data, 2, 100);
	MySPI_Stop(); // 片选拉高

	*val = rx_data[1];
}

// void imu963ra_read_acc_gyro_registers(uint8_t *val, uint8_t num)
// {
// 	MySPI_Start();
// 	HAL_SPI_TransmitReceive(&hspi2, val, val, num, 500);
// 	MySPI_Stop();
// }

uint8_t imu963ra_acc_gyro_self_check(void)
{
	uint8_t return_state = 0;
	uint8_t dat = 0;
	uint16_t timeout_count = 0;

	while (0x6B != dat) // 判断 ID 是否正确
	{
		if (IMU963RA_TIMEOUT_COUNT < timeout_count++)
		{
			return_state = 1;
			break;
		}
		imu963ra_read_acc_gyro_register(IMU963RA_WHO_AM_I, &dat);
		HAL_Delay(10);
	}
	return return_state;
}

uint8_t imu963ra_mag_self_check(void)
{
	uint8_t return_state = 0;
	uint8_t dat = 0;
	uint16_t timeout_count = 0;

	while (0xff != dat) // 判断 ID 是否正确
	{
		if (IMU963RA_TIMEOUT_COUNT < timeout_count++)
		{
			return_state = 1;
			break;
		}
		dat = imu963ra_read_mag_register(IMU963RA_MAG_ADDR, IMU963RA_MAG_CHIP_ID);
		HAL_Delay(10);
	}
	return return_state;
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     IMU963RA 作为 IIC 主机向磁力计写数据
// 参数说明     addr            目标地址
// 参数说明     reg             目标寄存器
// 参数说明     data            数据
// 返回参数     uint8_t           1-失败 0-成功
// 使用示例     imu963ra_write_mag_register(IMU963RA_MAG_ADDR, IMU963RA_MAG_CONTROL2, 0x80);
// 备注信息     内部调用
//-------------------------------------------------------------------------------------------------------------------
uint8_t imu963ra_write_mag_register(uint8_t addr, uint8_t reg, uint8_t data)
{
	uint8_t return_state = 0;
	uint16_t timeout_count = 0;
	uint8_t dat = 0;
	// i2c 0写1读
	//  addr=0x0d
	//  addr=0x1a
	addr = addr << 1;
	imu963ra_write_acc_gyro_register(IMU963RA_SLV0_CONFIG, 0x00);	 // 从机0配置清除
	imu963ra_write_acc_gyro_register(IMU963RA_SLV0_ADD, addr | 0);	 // 设置地磁计地址（注意这里需要设置8位的I2C地址） 0x2C
	imu963ra_write_acc_gyro_register(IMU963RA_SLV0_SUBADD, reg);	 // 需要写入的寄存器地址
	imu963ra_write_acc_gyro_register(IMU963RA_DATAWRITE_SLV0, data); // 需要写入的数据
	imu963ra_write_acc_gyro_register(IMU963RA_MASTER_CONFIG, 0x4C);	 // 仅在第一个周期启用通讯 开启上拉 I2C主机使能
	imu963ra_read_acc_gyro_register(IMU963RA_STATUS_MASTER, &dat);
	// 等待通讯成功
	while (0 == (0x80 & dat))
	{
		if (IMU963RA_TIMEOUT_COUNT < timeout_count++)
		{
			return_state = 1;
			break;
		}
		HAL_Delay(2);
	}
	return return_state;
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     IMU963RA 作为 IIC 主机向磁力计读数据
// 参数说明     addr            目标地址
// 参数说明     reg             目标寄存器
// 返回参数     uint8_t           读取的数据
// 使用示例     imu963ra_read_mag_register(IMU963RA_MAG_ADDR, IMU963RA_MAG_CHIP_ID);
// 备注信息     内部调用,调用一次可以读取一次磁力计数据1.2ms
//-------------------------------------------------------------------------------------------------------------------
uint8_t imu963ra_read_mag_register(uint8_t addr, uint8_t reg)
{
	uint16_t timeout_count = 0;
	uint8_t dat1 = 0;
	uint8_t dat2 = 0;
	addr = addr << 1;
	imu963ra_write_acc_gyro_register(IMU963RA_SLV0_ADD, addr | 1); // 设置地磁计地址（注意这里需要设置8位的I2C地址） 0x2C
	imu963ra_write_acc_gyro_register(IMU963RA_SLV0_SUBADD, reg);   // 需要读取的寄存器地址
	imu963ra_write_acc_gyro_register(IMU963RA_SLV0_CONFIG, 0x01);
	imu963ra_write_acc_gyro_register(IMU963RA_MASTER_CONFIG, 0x4C); // 仅在第一个周期启用通讯 开启上拉 I2C主机使能
	imu963ra_read_acc_gyro_register(IMU963RA_STATUS_MASTER, &dat1);
	// 等待通讯成功
	while (0 == (0x01 & dat1))
	{
		if (IMU963RA_TIMEOUT_COUNT < timeout_count++)
		{

			break;
		}
		HAL_Delay(2);
	}
	imu963ra_read_acc_gyro_register(IMU963RA_SENSOR_HUB_1, &dat2);
	return dat2; // 返回读取到的数据
}
// void imu963ra_read_acc_gyro_registers(uint8_t reg, uint8_t *data, uint32_t len)
// {
// 	MySPI_Start(); // SPI起始
// 	for (uint8_t i = 0; i < len; i++)
// 	{
// 		uint8_t current_reg = (reg + i) | IMU963RA_SPI_R; // 每次循环地址+1
// 		HAL_SPI_TransmitReceive(&hspi2, &current_reg, data + i, 1, 1000);
// 	}
// 	MySPI_Stop(); // SPI终止
// }
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     IMU963RA 作为 IIC 主机向磁力计自动写数据
// 参数说明     addr            目标地址
// 参数说明     reg             目标寄存器
// 返回参数     void
// 使用示例     imu963ra_connect_mag(IMU963RA_MAG_ADDR, IMU963RA_MAG_OUTX_L);
// 备注信息     内部调用
//-------------------------------------------------------------------------------------------------------------------
void imu963ra_connect_mag(uint8_t addr, uint8_t reg)
{
	addr = addr << 1;

	imu963ra_write_acc_gyro_register(IMU963RA_SLV0_ADD, addr | 1); // 设置地磁计地址（注意这里需要设置8位的I2C地址） 0x2C
	imu963ra_write_acc_gyro_register(IMU963RA_SLV0_SUBADD, reg);   // 需要读取的寄存器地址
	imu963ra_write_acc_gyro_register(IMU963RA_SLV0_CONFIG, 0x06);
	imu963ra_write_acc_gyro_register(IMU963RA_MASTER_CONFIG, 0x6C); // 仅在第一个周期启用通讯 开启上拉 I2C主机使能
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     获取 IMU963RA 加速度计数据
// 参数说明     void
// 返回参数     void
// 使用示例     imu963ra_get_acc();
// 备注信息     执行该函数后，直接查看对应的变量即可
//-------------------------------------------------------------------------------------------------------------------
void imu963ra_get_acc(void)
{

	uint8_t date[6] = {0};

	for (uint8_t i = 0; i < 6; i++)
	{
		imu963ra_read_acc_gyro_register(IMU963RA_OUTX_L_A + i, &date[i]);
	}

	imu963ra_acc_x = (int16_t)(((uint16_t)date[1] << 8 | date[0]));
	imu963ra_acc_y = (int16_t)(((uint16_t)date[3] << 8 | date[2]));
	imu963ra_acc_z = (int16_t)(((uint16_t)date[5] << 8 | date[4]));
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     获取IMU963RA陀螺仪数据
// 参数说明     void
// 返回参数     void
// 使用示例     imu963ra_get_gyro();
// 备注信息     执行该函数后，直接查看对应的变量即可
//-------------------------------------------------------------------------------------------------------------------
void imu963ra_get_gyro(void)
{

	uint8_t date[6] = {0};

	for (uint8_t i = 0; i < 6; i++)
	{
		imu963ra_read_acc_gyro_register(IMU963RA_OUTX_L_G + i, &date[i]);
	}

	imu963ra_gyro_x = (int16_t)((date[1] << 8) | date[0]);
	imu963ra_gyro_y = (int16_t)((date[3] << 8) | date[2]);
	imu963ra_gyro_z = (int16_t)((date[5] << 8) | date[4]);
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     获取 IMU963RA 磁力计数据
// 参数说明     void
// 返回参数     void
// 使用示例     imu963ra_get_mag();
// 备注信息     执行该函数后，直接查看对应的变量即可
//-------------------------------------------------------------------------------------------------------------------
void imu963ra_get_mag(void)
{
	uint8_t temp_status;
	imu963ra_write_acc_gyro_register(IMU963RA_FUNC_CFG_ACCESS, 0x40);
	imu963ra_read_acc_gyro_register(IMU963RA_STATUS_MASTER, &temp_status);
	if (0x01 & temp_status)
	{

		uint8_t date[6] = {0};

		for (uint8_t i = 0; i < 6; i++)
		{
			imu963ra_read_acc_gyro_register(IMU963RA_SENSOR_HUB_1 + i, &date[i]);
		}
		imu963ra_mag_x = (int16_t)(((uint16_t)date[1] << 8 | date[0]));
		imu963ra_mag_y = (int16_t)(((uint16_t)date[3] << 8 | date[2]));
		imu963ra_mag_z = (int16_t)(((uint16_t)date[5] << 8 | date[4]));
	}

	imu963ra_write_acc_gyro_register(IMU963RA_FUNC_CFG_ACCESS, 0x00);
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     初始化 IMU963RA
// 参数说明     void
// 返回参数     uint8_t           1-初始化失败 0-初始化成功
// 使用示例     imu963ra_init();
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
uint8_t imu963ra_init(void)
{
	uint8_t return_state = 0;
	HAL_Delay(10); // 上电延时

	do
	{
		imu963ra_write_acc_gyro_register(IMU963RA_FUNC_CFG_ACCESS, 0x00); // 关闭HUB寄存器访问
		imu963ra_write_acc_gyro_register(IMU963RA_CTRL3_C, 0x01);		  // 复位设备
		HAL_Delay(2);
		imu963ra_write_acc_gyro_register(IMU963RA_FUNC_CFG_ACCESS, 0x00); // 关闭HUB寄存器访问
		if (imu963ra_acc_gyro_self_check())
		{
			OLED_ShowString(64, 8, "error1", OLED_6X8);
			return_state = 1;
			break;
		}

		imu963ra_write_acc_gyro_register(IMU963RA_INT1_CTRL, 0x03);				  // 开启陀螺仪 加速度数据就绪中断
		imu963ra_write_acc_gyro_register(IMU963RA_CTRL1_XL, IMU963RA_GYR_SAMPLE); // 设置加速度计量程±8G以及数据输出速率52hz 以及加速度信息从第一级滤波器输出
																				  // IMU963RA_CTRL1_XL 寄存器
																				  // 设置为:0x30 加速度量程为:±2G      获取到的加速度计数据 除以16393，可以转化为带物理单位的数据，单位：g(m/s^2)
																				  // 设置为:0x38 加速度量程为:±4G      获取到的加速度计数据 除以8197， 可以转化为带物理单位的数据，单位：g(m/s^2)
																				  // 设置为:0x3C 加速度量程为:±8G      获取到的加速度计数据 除以4098， 可以转化为带物理单位的数据，单位：g(m/s^2)
																				  // 设置为:0x34 加速度量程为:±16G     获取到的加速度计数据 除以2049， 可以转化为带物理单位的数据，单位：g(m/s^2)
		// switch (IMU963RA_ACC_SAMPLE_DEFAULT)
		// {
		// default:
		// {
		// 	OLED_ShowString(64, 8, "error2", OLED_6X8);
		// 	return_state = 1;
		// }
		// break;
		// case IMU963RA_ACC_SAMPLE_SGN_2G:
		// {
		// 	imu963ra_write_acc_gyro_register(IMU963RA_CTRL1_XL, 0x30);
		// 	imu963ra_transition_factor[0] = 16393;
		// }
		// break;
		// case IMU963RA_ACC_SAMPLE_SGN_4G:
		// {
		// 	imu963ra_write_acc_gyro_register(IMU963RA_CTRL1_XL, 0x38);
		// 	imu963ra_transition_factor[0] = 8197;
		// }
		// break;
		// case IMU963RA_ACC_SAMPLE_SGN_8G:
		// {
		// 	imu963ra_write_acc_gyro_register(IMU963RA_CTRL1_XL, 0x3C);
		// 	imu963ra_transition_factor[0] = 4098;
		// }
		// break;
		// case IMU963RA_ACC_SAMPLE_SGN_16G:
		// {
		// 	imu963ra_write_acc_gyro_register(IMU963RA_CTRL1_XL, 0x34);
		// 	imu963ra_transition_factor[0] = 2049;
		// }
		// break;
		// }
		// if (1 == return_state)
		// {
		// 	break;
		// }

		// switch (IMU963RA_GYRO_SAMPLE_DEFAULT)
		// {
		// default:
		// {
		// 	OLED_ShowString(64, 8, "error3", OLED_6X8);
		// 	return_state = 1;
		// }
		// break;
		// case IMU963RA_GYRO_SAMPLE_SGN_125DPS:
		// {
		// 	imu963ra_write_acc_gyro_register(IMU963RA_CTRL2_G, 0x52);
		// 	imu963ra_transition_factor[1] = 228.6;
		// }
		// break;
		// case IMU963RA_GYRO_SAMPLE_SGN_250DPS:
		// {
		// 	imu963ra_write_acc_gyro_register(IMU963RA_CTRL2_G, 0x50);
		// 	imu963ra_transition_factor[1] = 114.3;
		// }
		// break;
		// case IMU963RA_GYRO_SAMPLE_SGN_500DPS:
		// {
		// 	imu963ra_write_acc_gyro_register(IMU963RA_CTRL2_G, 0x54);
		// 	imu963ra_transition_factor[1] = 57.1;
		// }
		// break;
		// case IMU963RA_GYRO_SAMPLE_SGN_1000DPS:
		// {
		// 	imu963ra_write_acc_gyro_register(IMU963RA_CTRL2_G, 0x58);
		// 	imu963ra_transition_factor[1] = 28.6;
		// }
		// break;
		// case IMU963RA_GYRO_SAMPLE_SGN_2000DPS:
		// {
		// 	imu963ra_write_acc_gyro_register(IMU963RA_CTRL2_G, 0x5C);
		// 	imu963ra_transition_factor[1] = 14.3;
		// }
		// break;
		// case IMU963RA_GYRO_SAMPLE_SGN_4000DPS:
		// {
		// 	imu963ra_write_acc_gyro_register(IMU963RA_CTRL2_G, 0x51);
		// 	imu963ra_transition_factor[1] = 7.1;
		// }
		// break;
		// }
		// if (1 == return_state)
		// {
		// 	break;
		// }
		imu963ra_write_acc_gyro_register(IMU963RA_CTRL2_G, IMU963RA_ACC_SAMPLE); // 设置陀螺仪计量程 ±2000dps 以及数据输出速率 208hz
																				 // IMU963RA_CTRL2_G 寄存器
																				 // 设置为:0x52 陀螺仪量程为:±125dps  获取到的陀螺仪数据除以228.6，   可以转化为带物理单位的数据，单位为：°/s
																				 // 设置为:0x50 陀螺仪量程为:±250dps  获取到的陀螺仪数据除以114.3，   可以转化为带物理单位的数据，单位为：°/s
																				 // 设置为:0x54 陀螺仪量程为:±500dps  获取到的陀螺仪数据除以57.1，    可以转化为带物理单位的数据，单位为：°/s
																				 // 设置为:0x58 陀螺仪量程为:±1000dps 获取到的陀螺仪数据除以28.6，    可以转化为带物理单位的数据，单位为：°/s
																				 // 设置为:0x5C 陀螺仪量程为:±2000dps 获取到的陀螺仪数据除以14.3，    可以转化为带物理单位的数据，单位为：°/s
																				 // 设置为:0x51 陀螺仪量程为:±4000dps 获取到的陀螺仪数据除以7.1，     可以转化为带物理单位的数据，单位为：°/s

		imu963ra_write_acc_gyro_register(IMU963RA_CTRL3_C, 0x44);  // 使能陀螺仪数字低通滤波器
		imu963ra_write_acc_gyro_register(IMU963RA_CTRL4_C, 0x02);  // 使能数字低通滤波器
		imu963ra_write_acc_gyro_register(IMU963RA_CTRL5_C, 0x00);  // 加速度计与陀螺仪四舍五入
		imu963ra_write_acc_gyro_register(IMU963RA_CTRL6_C, 0x00);  // 开启加速度计高性能模式 陀螺仪低通滤波 133hz
		imu963ra_write_acc_gyro_register(IMU963RA_CTRL7_G, 0x00);  // 开启陀螺仪高性能模式 关闭高通滤波
		imu963ra_write_acc_gyro_register(IMU963RA_CTRL9_XL, 0x01); // 关闭I3C接口

		imu963ra_write_acc_gyro_register(IMU963RA_FUNC_CFG_ACCESS, 0x40); // 开启HUB寄存器访问 用于配置地磁计
		imu963ra_write_acc_gyro_register(IMU963RA_MASTER_CONFIG, 0x80);	  // 复位I2C主机
		HAL_Delay(2);
		imu963ra_write_acc_gyro_register(IMU963RA_MASTER_CONFIG, 0x00); // 清除复位标志
		HAL_Delay(2);

		imu963ra_write_mag_register(IMU963RA_MAG_ADDR, IMU963RA_MAG_CONTROL2, 0x80); // 复位连接的外设
		//  0x0d 0x0a 0x80
		HAL_Delay(2);
		imu963ra_write_mag_register(IMU963RA_MAG_ADDR, IMU963RA_MAG_CONTROL2, 0x00);
		// 0x0d 0x0a 0x00
		HAL_Delay(2);

		if (imu963ra_mag_self_check())
		{
			OLED_ShowString(64, 8, "error4", OLED_6X8);
			return_state = 1;
			break;
		}

		// // IMU963RA_MAG_ADDR 寄存器
		// // 设置为 0x09 磁力计量程为 2G   获取到的磁力计数据除以 12000   可以转化为带物理单位的数据 单位 G(高斯)
		// // 设置为 0x19 磁力计量程为 8G   获取到的磁力计数据除以 3000    可以转化为带物理单位的数据 单位 G(高斯)
		// switch (IMU963RA_MAG_SAMPLE_DEFAULT)
		// {
		// default:
		// {
		// 	OLED_ShowString(64, 8, "error5", OLED_6X8);
		// 	return_state = 1;
		// }
		// break;
		// case IMU963RA_MAG_SAMPLE_2G:
		// {
		// 	imu963ra_write_mag_register(IMU963RA_MAG_ADDR, IMU963RA_MAG_CONTROL1, 0x09);
		// 	imu963ra_transition_factor[2] = 12000;
		// }
		// break;
		// case IMU963RA_MAG_SAMPLE_8G:
		// {
		// 	imu963ra_write_mag_register(IMU963RA_MAG_ADDR, IMU963RA_MAG_CONTROL1, 0x19);
		// 	imu963ra_transition_factor[2] = 3000;
		// }
		// break;
		// }
		// if (1 == return_state)
		// {
		// 	break;
		// }

		imu963ra_write_mag_register(IMU963RA_MAG_ADDR, IMU963RA_MAG_CONTROL1, IMU963RA_MAG_SAMPLE); // 设置磁力计量程8G 输出速率100hz 连续模式
																									// IMU963RA_MAG_ADDR 寄存器
																									// 设置为:0x19 磁力计量程为:8G     获取到的加速度计数据 除以3000， 可以转化为带物理单位的数据，单位：G(高斯)
																									// 设置为:0x09 磁力计量程为:2G     获取到的加速度计数据 除以12000，可以转化为带物理单位的数据，单位：G(高斯)

		imu963ra_write_mag_register(IMU963RA_MAG_ADDR, IMU963RA_MAG_FBR, 0x01);
		imu963ra_connect_mag(IMU963RA_MAG_ADDR, IMU963RA_MAG_OUTX_L);

		imu963ra_write_acc_gyro_register(IMU963RA_FUNC_CFG_ACCESS, 0x00); // 关闭HUB寄存器访问

		HAL_Delay(20); // 等待磁力计获取数据
	} while (0);

	return return_state;
}
